import rclpy
import math
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster  # 静态坐标发布
from geometry_msgs.msg import TransformStamped  # 几何相关的消息接口
from tf_transformations import quaternion_from_euler  # 欧拉角度到四元数的转换

class StaticTFBroadcaster(Node):
    def __init__(self):
        super().__init__('static_tf_broadcaster')
        self.static_broadcaster = StaticTransformBroadcaster(self) # 创建静态坐标发布对象
        self.publish_static_tf()

    def publish_static_tf(self):
        """
        发布静态坐标变换从base_link到camera_link的坐标变换
        """
        transfrom = TransformStamped()
        transfrom.header.frame_id = 'base_link'
        transfrom.child_frame_id = 'camera_link'
        transfrom.header.stamp = self.get_clock().now().to_msg()

        # 设置静态转换
        # 平移
        transfrom.transform.translation.x = 0.5
        transfrom.transform.translation.y = 0.6
        transfrom.transform.translation.z = 0.3
        # 旋转 q(X,Y,Z,W)
        q = quaternion_from_euler(math.radians(180), 0, 0)  # 绕x轴旋转180度
        transfrom.transform.rotation.x = q[0]
        transfrom.transform.rotation.y = q[1]
        transfrom.transform.rotation.z = q[2]
        transfrom.transform.rotation.w = q[3]
        # 发布静态坐标变换
        self.static_broadcaster.sendTransform(transfrom)
        self.get_logger().info(f'Publish static transform from base_link to camera_link:{transfrom}')

def main(args=None):
    rclpy.init(args=args)
    node = StaticTFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()

